/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
#define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_

#include <vector>
#include <costmap_2d/observation.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Point.h>
#include <base_local_planner/planar_laser_scan.h>

namespace base_local_planner
{
/** 轨迹控制器用来与世界交互的接口，而不管底层的世界模型是什么。
 * @class WorldModel
 * @brief An interface the trajectory controller uses to interact with the world regardless of the underlying world model.
 */
class WorldModel
{
public:
    /** 子类将实现这个方法来检查在给定位置和方向的足迹在世界坐标系上的合法性
     * @brief  Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
     * @param  position The position of the robot in world coordinates
     * @param  footprint The specification of the footprint of the robot in world coordinates
     * @param  inscribed_radius The radius of the inscribed circle of the robot
     * @param  circumscribed_radius The radius of the circumscribed circle of the robot
     * @return Positive if all the points lie outside the footprint, negative otherwise:
     *          -1 if footprint covers at least a lethal obstacle cell, or
     *          -2 if footprint covers at least a no-information cell, or
     *          -3 if footprint is partially or totally outside of the map
     */
    virtual double footprintCost(const geometry_msgs::Point &position, const std::vector<geometry_msgs::Point> &footprint,
                  double inscribed_radius, double circumscribed_radius) = 0;

    // inscribed_radius: 内切圆半径；circumscribed_radius：外切圆半径
    double footprintCost(double x, double y, double theta, const std::vector<geometry_msgs::Point> &footprint_spec,
                         double inscribed_radius = 0.0, double circumscribed_radius = 0.0)
    {
        double cos_th = cos(theta);
        double sin_th = sin(theta);
        // 将机器人的轮廓映射到路径点上
        std::vector<geometry_msgs::Point> oriented_footprint;
        for (const auto & i : footprint_spec)
        {
            geometry_msgs::Point new_pt;
            new_pt.x = x + (i.x * cos_th - i.y * sin_th);
            new_pt.y = y + (i.x * sin_th + i.y * cos_th);
            oriented_footprint.push_back(new_pt);
        }
        geometry_msgs::Point robot_position;
        robot_position.x = x;
        robot_position.y = y;
        if (inscribed_radius == 0.0)
        {
            costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius, circumscribed_radius);
        }
        return footprintCost(robot_position, oriented_footprint, inscribed_radius, circumscribed_radius);
    }

    /** 检查代价地图中是否有障碍物位于栅格化的足迹轮廓内
     * @brief  Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
     * @param  position The position of the robot in world coordinates
     * @param  footprint The specification of the footprint of the robot in world coordinates
     * @param  inscribed_radius The radius of the inscribed circle of the robot
     * @param  circumscribed_radius The radius of the circumscribed circle of the robot
     * @return Positive if all the points lie outside the footprint, negative otherwise
     */
    double footprintCost(const geometry_msgs::Point &position, const std::vector<geometry_msgs::Point> &footprint,
                         double inscribed_radius, double circumscribed_radius, double extra)
    {
        return footprintCost(position, footprint, inscribed_radius, circumscribed_radius);
    }

    /**
     * @brief  Subclass will implement a destructor
     */
    virtual ~WorldModel() {}

protected:
    WorldModel() {}
};

};
#endif
